#pragma once
#include <XnCppWrapper.h>
#include <math.h>
#include <time.h>

class Math
{
public:
	static const float PI;

	static XnPoint3D Point3D(float x = 0.0f, float y = 0.0f, float z = 0.0f) {
		XnPoint3D p;
		p.X = x; p.Y = y; p.Z = z;
		return p;
	};

	static float Point3_DistanceSquare(XnPoint3D p1, XnPoint3D p2) {
		float x_d = p1.X - p2.X;
		float y_d = p1.Y - p2.Y;
		float z_d = p1.Z - p2.Z;
		return x_d * x_d + y_d * y_d + z_d * z_d;
	};
	static float Point3_Distance(XnPoint3D p1, XnPoint3D p2) {
		return sqrt(Point3_DistanceSquare(p1, p2));
	};
	static XnPoint3D Point3_ReverseZ(XnPoint3D p) { return Point3D(p.X, p.Y, -p.Z); }

	static XnPoint3D Point3_MiddlePoint(XnPoint3D p1, XnPoint3D p2) { return Point3D((p1.X + p2.X) / 2.0f, (p1.Y + p2.Y) / 2.0f, (p1.Z + p2.Z) / 2.0f); }

	static XnPoint3D Point3_AddVector(XnPoint3D p, XnVector3D v) { return Point3D(p.X + v.X,p.Y + v.Y,p.Z + v.Z);}

	//----------------------
	static XnVector3D Vec3_Mult(XnVector3D v, float m) { return Vector3D(v.X * m, v.Y * m, v.Z * m); };

	static XnVector3D Vector3D(float x = 0.0f, float y = 0.0f, float z = 0.0f) {
		XnVector3D v;
		v.X = x; v.Y = y; v.Z = z;
		return v;
	};
	static XnVector3D GetVector3D(XnVector3D v1, XnVector3D v2) {
		XnVector3D v;
		v.X = v2.X - v1.X; v.Y = v2.Y - v1.Y; v.Z = v2.Z - v1.Z;
		return v;
	};
	static XnVector3D GetVector3D_P(XnPoint3D p1, XnPoint3D p2) {
		XnVector3D ve;
		ve.X = p2.X - p1.X; ve.Y = p2.Y - p1.Y; ve.Z = p2.Z - p1.Z;
		return ve;
	};
	static float Vec3_Norm(XnVector3D v) { return sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z); };
	static float Vec3_InnerProduct(XnVector3D v1, XnVector3D v2) { return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z; };
	static float Vec3_Theta(XnVector3D v1, XnVector3D v2) { return acos(Vec3_InnerProduct(v1, v2) / (Vec3_Norm(v1) * Vec3_Norm(v2))); };
	static float Vec3_ThetaDegree(XnVector3D v1, XnVector3D v2) { return RadToDeg(Vec3_Theta(v1, v2)); }
	static XnVector3D Vec3_Normalize(XnVector3D v) { 
		XnVector3D vn; 
		float norm = Vec3_Norm(v);
		vn.X = v.X / norm; vn.Y = v.Y / norm; vn.Z = v.Z / norm;
		return vn;
	};
	static XnVector3D Vec3_ReverseZ(XnVector3D v) { return Vector3D(v.X, v.Y, -v.Z); };

	//-----------------------
	static float RadToDeg(float rad) { return rad * 180.0f / PI; };
	static float DegToRad(float deg) { return deg * PI / 180.0f; };
	static int MaxMin(int value, int max, int min) { return (max < value) ? max : (min > value) ? min : value; }

	//-----------------------

	static float Rand(float min, float max) {
		if (!_randInit) { srand(clock()); _randInit = true; }
		return min + rand() * (max - min) / RAND_MAX;
	}

private:
	Math();
	Math(const Math&);
	static bool _randInit;
};

